#include "tiny_pid.h"

void PID_Init(PID *pid, float target, float kp, float ti, float td, float t)
{
    pid->Sv = target;

    pid->Kp = kp; //比例项
    pid->Ti = ti; //积分项
    pid->Td = td; //微分项
    pid->T = t;
    
    pid->Ek = 0;
    pid->Ek_1 = 0;
    pid->SEk = 0;
    
    pid->Iout = 0;
    pid->Pout = 0;
    pid->Dout = 0;
}

void PID_SetValue(PID *pid, float sv)
{
    pid->Sv = sv;
}

void PID_Set(PID *pid, float kp, float ti, float td)
{
    pid->Kp = kp; //比例项
    pid->Ti = ti; //积分项
    pid->Td = td; //微分项
    
    pid->SEk = 0;
    pid->Ek_1 = 0;
}

float PID_Calc(PID *pid, float pv)
{
#if 0
    float latest_diff;
    float out;

    pid->Pv = pv;

    pid->Ek = pid->Sv - pid->Pv; // 得到当前偏差值(ek)
    pid->Pout = pid->Kp * pid->Ek; // 比例输出(kp * ek)

    pid->SEk += pid->Ek; // 历史偏差总和(∑ej)
    pid->Iout = pid->Kp * pid->SEk / pid->Ti; // 积分输出(kp / ti * ∑ej)

    latest_diff = pid->Ek - pid->Ek_1; // 最近两次偏差之差
    pid->Dout = (pid->Kp * pid->Td * latest_diff) / pid->T; // 微分输出

    out = pid->Pout + pid->Iout + pid->Dout;
    if (out > 100)
        out = 100;
    if (out < 1)
        out = 1;

    pid->Ek_1 = pid->Ek; // 更新偏差

    return out;
#endif
    float DelEk;
    float ti, ki;
    float td;
    float kd;
    float out;

    pid->Pv = pv;

    pid->Ek = pid->Sv - pid->Pv; //得到当前偏差值
    pid->Pout = pid->Kp * pid->Ek; //比例输出

    if(pid->OUT >= 100)
    {
        if(pid->Ek < 0)
            pid->SEk += pid->Ek; //历史偏差总和
    }
    else if(pid->OUT <= 1)
    {
        if(pid->Ek > 0)
            pid->SEk += pid->Ek; //历史偏差总和
    }
    else
        pid->SEk += pid->Ek; //历史偏差总和
//    pid->SEk += pid->Ek; //历史偏差总和
    DelEk = pid->Ek - pid->Ek_1; //最近两次偏差之差

    ti = pid->T / pid->Ti;
    ki = ti * pid->Kp;
    pid->Iout = ki * pid->SEk * pid->Kp; //积分输出

    td = pid->Td / pid->T;
    kd = pid->Kp * td;
    pid->Dout = kd * DelEk; //微分输出

//    out = pid->Pout + pid->Iout + pid->Dout;
//    out = pid->Pout + pid->Iout;
    out = pid->Pout + pid->Iout + pid->Dout;
    if (out > 100)
        pid->OUT = 100;
    else if (out < 1)
        pid->OUT = 0;
    else
        pid->OUT = out;

    pid->Ek_1 = pid->Ek; //更新偏差

    return pid->OUT;
}






